#include<opencv2/opencv.hpp>
#include <algorithm>
#include <boost/timer.hpp>

#include "myslam/config.h"
#include "myslam/visual_odometry.h"
#include"myslam/g2o_types.h"
namespace myslam {

VisualOdometry::VisualOdometry():
    state_(INITIALIZING),ref_(nullptr),curr_(nullptr),map_(new Map),num_lost_(0),num_inliers_(0)
{
    num_of_features_ = Config::get<int>("number_of_features");
    scale_factor_ = Config::get<double>("scale_factor");
    level_pyramid_ = Config::get<int>("level_pyramid");
    match_ratio_ = Config::get<float>("match_ratio");
    max_num_lost_ = Config::get<float>("max_num_lost");
    min_inliers_ = Config::get<int> ("min_inliers");
    key_frame_min_rot = Config::get<double>("keyframes_rotation");
    Key_frame_min_trans = Config::get<double>("keyframe_translation");
    orb_ = cv::ORB::create(num_of_features_,scale_factor_,level_pyramid_);

}
VisualOdometry::~VisualOdometry()
{


}

bool VisualOdometry::addFrame(Frame::Ptr frame)
{
    switch(state_)
    {
    case INITIALIZING:

        cout<<"INITIALIZING ......"<<endl;
        state_ = OK;
        curr_= ref_ = frame;
        map_->insertKeyFrame(frame);
        extractKeyPoints();
        computeDescriptors();
        setRef3DPoints();
        cout<<"INITIALIZING is over"<<endl;
        break;

    case OK:

        curr_= frame;
        extractKeyPoints();
        computeDescriptors();

        featureMatching();
        poseEstimationPnp();
        cout<<"poseEstimationPnp ......"<<endl;
        if(checkEstimationPose()==true)
//         if(1)
        {

            curr_->T_c_w_ = T_c_r_estimated_*ref_->T_c_w_;
            curr_->T_c_w_BA_ = T_c_r_BA_*ref_->T_c_w_BA_;
            ref_ = curr_;
            setRef3DPoints();
            num_lost_=0;
//            if(checkKeyFrame()==true)
//            {
//                addKeyFrame();
//            }
        }
        else
        {
            num_lost_++;
            curr_->T_c_w_ = ref_->T_c_w_;
            curr_->T_c_w_BA_ = ref_->T_c_w_BA_;
            cout<<"位姿过大或太小次数："<<num_lost_<<endl;
//           if(num_lost_>max_num_lost_)
//           {
//              state_ = LOST;

//           }



//            return false;
        }

        break;

    case LOST:

        cout<<"vo has lost......"<<endl;

        return false;

    }

    return true;
}




void VisualOdometry::extractKeyPoints()
{
    orb_->detect(curr_->color_,keypoints_curr_);
    cout<<"extractKeypoints() is ok!"<<endl;



}

void VisualOdometry::computeDescriptors()
{
    orb_->compute(curr_->color_,keypoints_curr_,descriptors_curr_);
    cout<<"computeDescriptors() is ok!"<<endl;
}
void VisualOdometry::featureMatching()
{
    vector<cv::DMatch>matches;

    cv::BFMatcher matcher(cv::NORM_HAMMING);
    matcher.match(descriptors_ref_,descriptors_curr_,matches);

    float min_dis = std::max_element(
                matches.begin(),matches.end(),[](const cv::DMatch&m1,cv::DMatch&m2)
    {
        return m1.distance<m2.distance;
    })->distance;
    feature_matches_.clear();
    for(cv::DMatch &m :matches)
    {
        if(m.distance <max<float> (min_dis*match_ratio_,30.0))
        {
            feature_matches_.push_back(m);
        }
    }
    cout<<"good matches :"<<feature_matches_.size()<<endl;


}

void  VisualOdometry::setRef3DPoints()
{
    // cout<<"setRef3DPoints is ok1 !"<<endl;
    pts_3d_ref_.clear();
    descriptors_ref_ = Mat();
    // cout<<"setRef3DPoints is ok12 !"<<endl;
    for(size_t i = 0;i<keypoints_curr_.size();i++)
    {
        double d = ref_->findDepth(keypoints_curr_[i]);

        if(d>0)
        {
            //  cout<<"setRef3DPoints is ok12 !"<<keypoints_curr_[i].pt.x<<","<<keypoints_curr_[i].pt.y<<"i="<<i<<endl;

            Vector3d p_cam = ref_->camera_->pixel2camera(
                        Vector2d(keypoints_curr_[i].pt.x,keypoints_curr_[i].pt.y),d
                        );

            //  cout<<"depth="<<d<<endl;
            pts_3d_ref_.push_back(cv::Point3f(p_cam(0,0),p_cam(1,0),p_cam(2,0)));
            descriptors_ref_.push_back(descriptors_curr_.row(i));

        }
    }

    //  cout<<"setRef3DPoints is ok !"<<endl;
}


void VisualOdometry::poseEstimationPnp()
{
    static int c = 0;

    vector<cv::Point3f> pts3d;
    vector<cv::Point2f> pts2d;

    for(cv::DMatch m:feature_matches_)
    {
        pts3d.push_back(pts_3d_ref_[m.queryIdx]);
        pts2d.push_back(keypoints_curr_[m.trainIdx].pt);
    }

    Mat K = (cv::Mat_<double>(3,3)<<
             ref_->camera_->fx_,0,ref_->camera_->cx_,
             0,ref_->camera_->fy_,ref_->camera_->cy_,
             0,0,1
             );

    Mat rvec,tvec,inliers;

    cv::solvePnPRansac(pts3d,pts2d,K,Mat(),rvec,tvec,false,100,8.0,0.99,inliers);
    num_inliers_ = inliers.rows;

    cout<<"pnp inliers: "<<  num_inliers_<<endl;

    cout<<"c = "<<c++<<endl;
    Mat rotation_matrix = Mat(3,3,CV_64FC1,cv::Scalar::all(0));
    Rodrigues(rvec,rotation_matrix);
    cout<<"rotation_matrix is "<<endl<<rotation_matrix<<endl;

    Eigen::Matrix3d tmp;
    tmp<<rotation_matrix.at<double>(0,0),rotation_matrix.at<double>(0,1),rotation_matrix.at<double>(0,2),
            rotation_matrix.at<double>(1,0),rotation_matrix.at<double>(1,1),rotation_matrix.at<double>(1,2),
            rotation_matrix.at<double>(2,0),rotation_matrix.at<double>(2,1),rotation_matrix.at<double>(2,2);

    T_c_r_estimated_ = Sophus::SE3d(tmp,Vector3d(tvec.at<double>(0,0),tvec.at<double>(0,1),tvec.at<double>(0,2)));
    cout<<"estimated_ = "<<endl<<T_c_r_estimated_.matrix()<<endl;


    //BA
    // using bundle adjustment to optimize the pose
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,2>> Block;
    typedef g2o::LinearSolverDense<Block::PoseMatrixType> LinearSolverType;
    auto solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<Block>(g2o::make_unique<LinearSolverType>()));


    //   Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
    // Block* solver_ptr = new Block( linearSolver );
    //g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );

    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    pose->setId ( 0 );
    pose->setEstimate ( g2o::SE3Quat (
                            T_c_r_estimated_.rotationMatrix(),
                            T_c_r_estimated_.translation()

                            ) );
    optimizer.addVertex ( pose );

    // edges
    for ( int i=0; i<inliers.rows; i++ )
    {
        int index = inliers.at<int>(i,0);
        // 3D -> 2D projection
        EdgeProjectXYZ2UVPoseOnly* edge = new EdgeProjectXYZ2UVPoseOnly();
        edge->setId(i);
        edge->setVertex(0, pose);
        edge->camera_ = curr_->camera_.get();
        edge->point_ = Vector3d( pts3d[index].x, pts3d[index].y, pts3d[index].z );
        edge->setMeasurement( Vector2d(pts2d[index].x, pts2d[index].y) );
        edge->setInformation( Eigen::Matrix2d::Identity() );
        optimizer.addEdge( edge );
    }

    optimizer.initializeOptimization();
    optimizer.optimize(50);

    T_c_r_BA_ = Sophus::SE3d(pose->estimate().rotation().matrix(),pose->estimate().translation());
    cout<<"optimization is finished!!"<<endl;
    cout<<"旋转:"<<pose->estimate().rotation().matrix()<<endl;
    cout<<"平移:"<<pose->estimate().translation().transpose()<<endl;
    cout<<"estimated_BA = "<<endl<<T_c_r_BA_.matrix()<<endl;

    cout<<"------------^_^--------------"<<endl;

}


bool VisualOdometry::checkEstimationPose()
{
    if(num_inliers_ <min_inliers_)
    {
        cout<<"reject because inliers is too small:"<<num_inliers_<<endl;
        return false;

    }
    Sophus::Vector6d d = T_c_r_estimated_.log();
    if(d.norm() >4)
    {
        cout<<"reject because motion is too large :"<<d.norm()<<endl;
        return false;
    }

    cout<<"checkEstimatedPose is passed!!!"<<endl;
    return true;
}

bool VisualOdometry::checkKeyFrame()
{
    Sophus::Vector6d d = T_c_r_estimated_.log();
    Vector3d trans = d.head<3>();
    Vector3d rot = d.tail<3>();
    if(rot.norm() >key_frame_min_rot || trans.norm() > Key_frame_min_trans)
        return true;
    return false;

}
void VisualOdometry::addKeyFrame()
{

    cout<<"adding a key-frame"<<endl;
    //  map_->insertKeyFrame(curr_);
}









} //namespace myslam
